;;==========================================================================
;; Copyright (c) 2000-2008,  Elastos, Inc.  All Rights Reserved.
;;==========================================================================

#include <bsp/bspcfg.h>
#include <kcfg.h>

    AREA |.text|,ALIGN=2,CODE, READONLY

    IMPORT bsp

;
;   KrnSetup()
;

    IMPORT  __KernelSetup
    EXPORT  KrnSetup

KrnSetup
    str     lr, OldLR
    str     sp, OldSP
    str     fp, OldFP
    ldr     lr, RetAdr
    b       __KernelSetup

OldLR       DCD 0
OldSP       DCD 0
OldFP       DCD 0
RetAdr      DCD __KernelSetupExit

__KernelSetupExit
    ldr     r3, =KCONFIG_KERNEL_BASE
    ldr     r4, =bsp
    ldr     r4, [r4, #8] ; r4 = bsp.paKernelPhysBase
    sub     r3, r3, r4

    adr     r0, OldLR
    ldr     r1, [r0]
    add     r1, r1, r3
    mov     lr, r1

    adr     r0, OldSP
    ldr     r1, [r0]
    add     r1, r1, r3
    mov     sp, r1

    adr     r0, OldFP
    ldr     r1, [r0]
    add     r1, r1, r3
    mov     fp, r1

    mov     pc, lr


;
;   SetupMMU()
;
    EXPORT  SetupMMU

SetupMMU
    str     lr, OldLR2
    str     sp, OldSP2
    str     fp, OldFP2
    ldr     lr, RetAdr2
    b       __SetupMMU

OldLR2      DCD 0
OldSP2      DCD 0
OldFP2      DCD 0
RetAdr2     DCD __SetupMMUExit

__SetupMMUExit
    ldr     r3, =KCONFIG_KERNEL_BASE
    ldr     r4, =bsp
    ldr     r4, [r4, #8] ; r4 = bsp.paKernelPhysBase
    sub     r3, r3, r4

    adr     r0, OldLR2
    ldr     r1, [r0]
    add     r1, r1, r3
    mov     lr, r1

    adr     r0, OldSP2
    ldr     r1, [r0]
    add     r1, r1, r3
    mov     sp, r1

    adr     r0, OldFP2
    ldr     r1, [r0]
    add     r1, r1, r3
    mov     fp, r1

    mov     pc, lr


;
;   void __SetupMMU(
;       physaddr_t paKernelPhysBase, physaddr_t paKernelImagePhysBase,
;       physaddr_t paMemoryMappedIoBase, physaddr_t paMemoryMappedIoLimit)
;
    IMPORT  SetupPaging
    IMPORT  AdjustPaging

__SetupMMU
    mov     ip, sp
    stmdb   sp!, {fp, ip, lr, pc}
    sub     fp, ip, #4
    sub     sp, sp, #16
    str     r0, [fp, #-16]
    str     r1, [fp, #-20]
    str     r2, [fp, #-24]
    str     r3, [fp, #-28]
    bl      SetupPaging

    ldr     pc, NextAddr

NextAddr    DCD _Next

_Next
    ldr     r3, =KCONFIG_KERNEL_BASE
    ldr     r4, =bsp
    ldr     r4, [r4, #8] ; r4 = bsp.paKernelPhysBase
    sub     r3, r3, r4
    add     sp, sp, r3
    add     fp, fp, r3

    ldr     r0, [fp, #-16]
    bl      AdjustPaging

    sub     sp, fp, #12
    ldmia   sp, {fp, sp, pc}


;
;   void FlushCache()
;
#if defined(_xscale)
#define RESERVED_CACHEABLE_REGION   0xffe00000
#endif

    IMPORT  SaveFlagsAndCli
    IMPORT  RestoreIF
    EXPORT  FlushCache

FlushCache

    bl      SaveFlagsAndCli
    mov     r12, r0

#if defined(_arm720)

    mov     r0, #0
    mcr     p15, 0, r0, c7, c7, 0

#elif defined(_arm920)

    mov     r1, #3 << 5                 // 4 segments
Label1
    orr     r2, r1, #63 << 26           // 64 entries
Label2
    mcr     p15, 0, r2, c7, c14, 2      // clean & invalidate D index
    subs    r2, r2, #1 << 26
    bcs     Label2                      // entries 63 to 0
    subs    r1, r1, #1 << 5
    bcs     Label1                      // segments 3 to 0
    mov     r0, #0
    mcr     p15, 0, r0, c7, c10, 4      // drain write buffer
    mcr     p15, 0, r0, c7, c5, 0       // BUG: need invalidate I cache?

#elif defined(_xscale)

    // Global Clean or Invalidate The Data Cache
    mov     r0, #2048
    ldr     r1, =RESERVED_CACHEABLE_REGION

Label1
    mcr     p15, 0, r1, c7, c2, 5       // Allocate a line at the virtual
                                        //  address specified by r1

    add     r1, r1, #32                 // Increment the address in r1 to
                                        // the next cache line
    subs    r0, r0, #1                  // Decrement loop count
    bne     Label1

    mcr     p15, 0, r0, c7, c10, 4      // drain write (& fill) buffer

    mcr     p15, 0, r0, c7, c7, 0       // invalidate D & I cache

    mrc     p15, 0, r0, c2, c0, 0       // arbitrary read of cp15
    mov     r0, r0                      // wait for it
    sub     pc, pc, #4

#else
#error Unknown architecture variant
#endif

    mov     r0, r12
    bl      RestoreIF

    mov     pc, lr


;
;   void FlushAllTlbs()
;
    EXPORT  FlushAllTlbs

FlushAllTlbs

#if defined(_arm720)

    mov     r0, #0
    mcr     p15, 0, r0, c8, c7, 0           // invalidate unified TLB

#elif defined(_arm920) || defined(_xscale)

    mov     r0, #0
    mcr     p15, 0, r0, c7, c10, 4          // drain write buffer
    mcr     p15, 0, r0, c8, c7, 0           // invalidate I & D TLBs

#else
#error Unknown architecture variant
#endif

    mov     pc, lr


;
;   void FlushTlb(virtaddr_t va)
;
    EXPORT  FlushTlb

FlushTlb

#if defined(_arm720)

    mcr     p15, 0, r0, c8, c7, 1

#elif defined(_arm920) || defined(_xscale)

    mov     r2, #0
    mcr     p15, 0, r2, c7, c10, 4      // drain write buffer

    // BUG: need invalidate I TLB in any case?
    mcr     p15, 0, r0, c8, c6, 1       // invalidate D TLB
    mcr     p15, 0, r0, c8, c5, 1       // invalidate I TLB

#else
#error Unknown architecture variant
#endif

    mov     pc, lr


;
;   void SetPageDirAddress(physaddr_t pagedir)
;
    EXPORT  SetPageDirAddress

SetPageDirAddress
    mcr     p15, 0, r0, c2, c0, 0

    mov     pc, lr


;
;   uint32_t GetFaultStatus()
;
    EXPORT  GetFaultStatus

GetFaultStatus
    mrc     p15, 0, r0, c5, c0, 0

    mov     pc, lr


;
;   void SetFaultStatus(uint32_t fs)
;
    EXPORT  SetFaultStatus

SetFaultStatus
    mcr     p15, 0, r0, c5, c0, 0

    mov     pc, lr


;
;   virtaddr_t GetFaultAddress()
;
    EXPORT  GetFaultAddress

GetFaultAddress
    mrc     p15, 0, r0, c6, c0, 0

    mov     pc, lr


;
;   void SetFaultAddress(virtaddr_t fa)
;
    EXPORT  SetFaultAddress

SetFaultAddress
    mcr     p15, 0, r0, c6, c0, 0

    mov     pc, lr


;
;   void EnableMMU(uint32_t cr3, physaddr_t pagedir, uint32_t cr1)
;
    EXPORT  EnableMMU

EnableMMU
    mov     ip, sp
    stmdb   sp!, {fp, ip, lr, pc}
    sub     fp, ip, #4
    sub     sp, sp, #12
    str     r0, [fp, #-16]
    str     r1, [fp, #-20]
    str     r2, [fp, #-24]

    bl      FlushAllTlbs

    ldr     r0, [fp, #-16]
    mcr     p15, 0, r0, c3, c0, 0
    ldr     r1, [fp, #-20]
    mcr     p15, 0, r1, c2, c0, 0
    ldr     r2, [fp, #-24]
    mcr     p15, 0, r2, c1, c0, 0
    nop
    nop

#if defined(_xscale)
    mrc     p15, 0, r0, c2, c0, 0       // arbitrary read of cp15
    mov     r0, r0                      // wait for it
    sub     pc, pc, #4
#endif
    sub     sp, fp, #12
    ldmia   sp, {fp, sp, pc}


    END
